
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_attitude_control_multi.h
  * @author     baiyang
  * @date       2021-8-9
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "mc_attitude_control.h"

#include <motor/gp_motors_multicopter.h>
/*-----------------------------------macro------------------------------------*/
// default rate controller PID gains
#ifndef AC_ATC_MULTI_RATE_RP_P
  #define AC_ATC_MULTI_RATE_RP_P           0.135f
#endif
#ifndef AC_ATC_MULTI_RATE_RP_I
  #define AC_ATC_MULTI_RATE_RP_I           0.135f
#endif
#ifndef AC_ATC_MULTI_RATE_RP_D
  #define AC_ATC_MULTI_RATE_RP_D           0.0036f
#endif
#ifndef AC_ATC_MULTI_RATE_RP_IMAX
  #define AC_ATC_MULTI_RATE_RP_IMAX         0.5f
#endif
#ifndef AC_ATC_MULTI_RATE_RP_FILT_HZ
  #define AC_ATC_MULTI_RATE_RP_FILT_HZ      20.0f
#endif
#ifndef AC_ATC_MULTI_RATE_YAW_P
  #define AC_ATC_MULTI_RATE_YAW_P           0.180f
#endif
#ifndef AC_ATC_MULTI_RATE_YAW_I
  #define AC_ATC_MULTI_RATE_YAW_I           0.018f
#endif
#ifndef AC_ATC_MULTI_RATE_YAW_D
  #define AC_ATC_MULTI_RATE_YAW_D           0.0f
#endif
#ifndef AC_ATC_MULTI_RATE_YAW_IMAX
  #define AC_ATC_MULTI_RATE_YAW_IMAX        0.5f
#endif
#ifndef AC_ATC_MULTI_RATE_YAW_FILT_HZ
  #define AC_ATC_MULTI_RATE_YAW_FILT_HZ     2.5f
#endif

/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    Attitude_ctrl   parent;
   
    MotorsMC_HandleTypeDef* _motors;
    PID_ctrl                _pid_rate_roll;
    PID_ctrl                _pid_rate_pitch;
    PID_ctrl                _pid_rate_yaw;

    Param_float             _thr_mix_man;     // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle)
    Param_float             _thr_mix_min;     // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
    Param_float             _thr_mix_max;     // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
} AttitudeMulti_ctrl;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// 
void   attctrl_multi_init(Attitude_ctrl * att_ctrl, Motors_HandleTypeDef* motors, float dt);

// run lowest level body-frame rate controller and send outputs to the motors
void   attctrl_rate_controller_run(Attitude_ctrl * att_ctrl);

// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
void attctrl_update_throttle_rpy_mix(Attitude_ctrl * att_ctrl);

// sanity check parameters.  should be called once before takeoff
void attctrl_parameter_sanity_check(Attitude_ctrl * att_ctrl);

// Update Alt_Hold angle maximum
void attctrl_update_althold_lean_angle_max(Attitude_ctrl * att_ctrl, float throttle_in);

void attctrl_set_throttle_out(Attitude_ctrl * att_ctrl, float throttle_in, bool apply_angle_boost, float filter_cutoff);
void attctrl_set_throttle_mix_max(Attitude_ctrl * att_ctrl, float ratio);

// returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1
float attctrl_get_throttle_boosted(Attitude_ctrl * att_ctrl, float throttle_in);

// returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1
float attctrl_get_throttle_avg_max(Attitude_ctrl * att_ctrl, float throttle_in);

// pid accessors
static inline PID_ctrl* attctrl_get_rate_roll_pid(Attitude_ctrl * att_ctrl) { return &((AttitudeMulti_ctrl*)att_ctrl)->_pid_rate_roll; }
static inline PID_ctrl* attctrl_get_rate_pitch_pid(Attitude_ctrl * att_ctrl) { return &((AttitudeMulti_ctrl*)att_ctrl)->_pid_rate_pitch; }
static inline PID_ctrl* attctrl_get_rate_yaw_pid(Attitude_ctrl * att_ctrl) { return &((AttitudeMulti_ctrl*)att_ctrl)->_pid_rate_yaw; }

// set desired throttle vs attitude mixing (actual mix is slewed towards this value over 1~2 seconds)
//  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
//  has no effect when throttle is above hover throttle
static inline void attctrl_set_throttle_mix_min(Attitude_ctrl * att_ctrl) { att_ctrl ->_throttle_rpy_mix_desired = ((AttitudeMulti_ctrl*)att_ctrl)->_thr_mix_min; }
static inline void attctrl_set_throttle_mix_man(Attitude_ctrl * att_ctrl) { att_ctrl ->_throttle_rpy_mix_desired = ((AttitudeMulti_ctrl*)att_ctrl)->_thr_mix_man; }
static inline void attctrl_set_throttle_mix_value(Attitude_ctrl * att_ctrl, float value) { att_ctrl ->_throttle_rpy_mix_desired = att_ctrl->_throttle_rpy_mix = value; }
static inline float attctrl_get_throttle_mix(Attitude_ctrl * att_ctrl) { return att_ctrl->_throttle_rpy_mix; }

// are we producing min throttle?
bool attctrl_is_throttle_mix_min(Attitude_ctrl * att_ctrl);


void attctrl_reset_rate_controller_I_terms(Attitude_ctrl * att_ctrl);

// reset rate controller I terms smoothly to zero in 0.5 seconds
void attctrl_reset_rate_controller_I_terms_smoothly(Attitude_ctrl * att_ctrl);

// 更新姿态和陀螺仪数据，用于姿态控制
void attctrl_updata_att_data(Attitude_ctrl * att_ctrl);

// 更新姿态控制器相关参数
void attctrl_update_param(Attitude_ctrl * att_ctrl);

// 
void attctrl_publish_msg(Attitude_ctrl * att_ctrl);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



